/*
 * Gait.h
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#ifndef GAIT_H_
#define GAIT_H_

#include "Robot.h"
#include "Leg.h"
#include "Link.h"
#include "Matrix.h"
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>

using std::endl;
using std::cout;

class Gait {
private:
    	int logbool;
    	int gaitmotion;
	double height;
	double width;
	double length;
	int type;
	double legmax;
	int offset0;
	int offset1;
	int offset2;
	int offset3;
	int offset4;
	int duty;
	double speed;
	bool walking;

public:
	Robot* rbe;

	Gait(Robot* ST, int gaitnum, double setspeed) {
		rbe = ST;
		height = .2;
		width = .2;
		length = .2;
		duty = 0;
		offset0 = 0;
		offset1 = 0;
		offset2 = 0;
		offset3 = 0;
		offset4 = 0;
		Gait::setType(gaitnum);
		legmax = sqrt(pow(LINK1LENGTH + LINK2LENGTH + LINK3LENGTH, 2) - pow(length
				/ 2, 2)) - .2;
		speed = setspeed;
		walking = 1;
		gaitmotion = 1;
		logbool = 0;
	}

	virtual ~Gait() {
	}

	int getDuty() const { return duty; }
    	double getHeight() const { return height; }
    	double getLegmax() const { return legmax; }
    	double getLength() const { return length; }
    	int getOffset0() const { return offset0; }
    	int getOffset1() const { return offset1; }
    	int getOffset2() const { return offset2; }
    	int getOffset3() const { return offset3; }
   	int getOffset4() const { return offset4; }
    	int getType() const { return type; }
    	bool getWalking() const { return walking; }
    	double getWidth() const { return width; }
    	void setDuty(int duty) {  this->duty = duty; }
    	void setHeight(double height) { this->height = height; }
    	void setLegmax(double legmax) { this->legmax = legmax; }
    	void setLength(double length) { this->length = length; }
    	void setOffset0(int offset0) { this->offset0 = offset0; }
    	void setOffset1(int offset1) { this->offset1 = offset1; }
    	void setOffset2(int offset2) { this->offset2 = offset2; }
    	void setOffset3(int offset3) { this->offset3 = offset3; }
    	void setOffset4(int offset4) { this->offset4 = offset4; }
    	void setType(int type) { 
		this->type = type;
		switch (type) {
		//Lateral Sequence Walking
		case 1:
			duty = 25;
			offset0 = 0;
			offset1 = 25;
			offset2 = 50;
			offset3 = 75;
			offset4 = offset3 + duty;
			break;
			//Walking Trot
		case 2:
			duty = 40;
			offset0 = 10;
			offset1 = 10;
			offset2 = 60;
			offset3 = 60;
			offset4 = offset3 + duty;
			break;
			//Running Trot
		case 3:
			duty = 60;
			offset0 = 90;
			offset1 = 90;
			offset2 = 40;
			offset3 = 40;
			offset4 = offset0 + duty;
			break;
			//Rotary Gallop
		case 4:
			duty = 70;
			offset0 = 90;
			offset1 = 40;
			offset2 = 80;
			offset3 = 30;
			offset4 = offset0 + duty;
			break;
			//Traverse Gallop
		case 5:
			duty = 75;
			offset0 = 0;
			offset1 = 40;
			offset2 = 80;
			offset3 = 25;
			offset4 = offset2 + duty;
			break;
			//Bound
		case 6:
			duty = 70;
			offset0 = 30;
			offset1 = 65;
			offset2 = 30;
			offset3 = 65;
			offset4 = offset1 + duty;
			break;
		}
 	}
    	void setWalking(bool walking) { this->walking = walking; }
    	void setWidth(double width) { this->width = width; }
    	double getSpeed() const { return speed; }
    	void setSpeed(double speed) { this->speed = speed; }

    	void printInfo() {
		cout << "Gait Height: " << Gait::getHeight() << endl;
		cout << "Gait Length: " << Gait::getLength() << endl;
		cout << "Gait Width: " << Gait::getWidth() << endl;
		cout << "Gait Type: " << Gait::getType() << endl;
		cout << "Gait Duty: " << Gait::getDuty() << endl;
		cout << "Gait Offset0: " << Gait::getOffset0() << endl;
		cout << "Gait Offset1: " << Gait::getOffset1() << endl;
		cout << "Gait Offset2: " << Gait::getOffset2() << endl;
		cout << "Gait Offset3: " << Gait::getOffset3() << endl;
		cout << "Gait Offset4: " << Gait::getOffset4() << endl;
		cout << "Gait Legmax: " << Gait::getLegmax() << endl;
		cout << "Gait Speed: " << Gait::getSpeed() << endl;
	}

    	int getGaitmotion() const { return gaitmotion; }
    	void setGaitmotion(int gaitmotion) { this->gaitmotion = gaitmotion; }

    	void calculateGait() {
		//Leg 1 Motion Planning
		if ((gaitmotion > offset0) && (gaitmotion <= (offset0 + duty))) {
			//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
			double i0 = ((double) gaitmotion - (double) offset0) * (360.0
					/ (double) duty);
			rbe->legs[0].setDesiredx(rbe->legs[0].getDesiredx()
					+ (double) (rbe->getFl()) * width / (double) duty); // Calculates the desired x
			rbe->legs[0].setDesiredy(rbe->legs[0].getDesiredy()
					+ (double) (rbe->getVl()) * length / (double) duty);
			rbe->legs[0].setDesiredz(rbe->legs[0].getDesiredz()
					- (double) (rbe->getHl()) * sin(i0 * (double) (PI / 180.0))
							* 2.0 / (double) duty * height);
			if (walking == 1) {
				//weight on body = 0
			} else {
				//weight on body = -264
			}
		}
		if ((gaitmotion < offset0) || (gaitmotion >= (offset0 + duty))) {
			rbe->legs[0].setDesiredx(rbe->legs[0].getDesiredx()
					- (double) rbe->getFl() * width / ((double) offset4
							- (double) duty)); // Calculates the desired x
			rbe->legs[0].setDesiredy(rbe->legs[0].getDesiredy()
					- (double) rbe->getVl() * length / ((double) offset4
							- (double) duty));
			rbe->legs[0].setDesiredz(rbe->legs[0].getDesiredz());
			//weight on body = -264
		}

		//Leg 2 Motion Planning
		if ((gaitmotion > offset2) && (gaitmotion <= (offset2 + duty))) {
			//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
			double i2 = ((double) gaitmotion - (double) offset2) * (360.0
					/ (double) duty);
			rbe->legs[1].setDesiredx(rbe->legs[1].getDesiredx()
					+ (double) rbe->getFl() * width / (double) duty); // Calculates the desired x
			rbe->legs[1].setDesiredy(rbe->legs[1].getDesiredy()
					+ (double) rbe->getVl() * length / (double) duty);
			rbe->legs[1].setDesiredz(rbe->legs[1].getDesiredz()
					- (double) (rbe->getHl()) * sin(i2 * (double) (PI / 180.0))
							* 2.0 / (double) duty * height);
			if (walking == 1) {
				//weight on body = 0
			} else {
				//weight on body = -264
			}
		}
		if ((gaitmotion < offset2) || (gaitmotion >= (offset2 + duty))) {
			rbe->legs[1].setDesiredx(rbe->legs[1].getDesiredx()
					- (double) rbe->getFl() * width / ((double) offset4
							- (double) duty)); // Calculates the desired x
			rbe->legs[1].setDesiredy(rbe->legs[1].getDesiredy()
					- (double) rbe->getVl() * length / ((double) offset4
							- (double) duty));
			rbe->legs[1].setDesiredz(rbe->legs[1].getDesiredz());
			//weight on body = -264
		}

		//Leg 3 Motion Planning
		if ((gaitmotion > offset1) && (gaitmotion <= (offset1 + duty))) {
			//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
			double i1 = ((double) gaitmotion - (double) offset1) * (360.0
					/ (double) duty);
			//cout << "Angle: " << i1 <<  endl;
			rbe->legs[2].setDesiredx(rbe->legs[2].getDesiredx()
					+ (double) rbe->getBl() * width / (double) duty); // Calculates the desired x
			rbe->legs[2].setDesiredy(rbe->legs[2].getDesiredy()
					+ (double) rbe->getVl() * length / (double) duty);
			rbe->legs[2].setDesiredz(rbe->legs[2].getDesiredz()
					- (double) (rbe->getHl()) * sin(i1 * (double) (PI / 180.0))
							* 2.0 / (double) duty * height);
			if (walking == 1) {
				//weight on body = 0
			} else {
				//weight on body = -264
			}
		}
		if ((gaitmotion < offset1) || (gaitmotion >= (offset1 + duty))) {
			rbe->legs[2].setDesiredx(rbe->legs[2].getDesiredx()
					- (double) rbe->getBl() * width / ((double) offset4
							- (double) duty)); // Calculates the desired x
			rbe->legs[2].setDesiredy(rbe->legs[2].getDesiredy()
					- (double) rbe->getVl() * length / ((double) offset4
							- (double) duty));
			rbe->legs[2].setDesiredz(rbe->legs[2].getDesiredz());
			//weight on body = -264
		}

		//Leg 4 Motion Planning
		if ((gaitmotion > offset3) && (gaitmotion <= (offset3 + duty))) {
			//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
			double i3 = ((double) gaitmotion - (double) offset3) * (360.0
					/ (double) duty);
			rbe->legs[3].setDesiredx(rbe->legs[3].getDesiredx()
					+ (double) rbe->getBl() * width / (double) duty); // Calculates the desired x
			rbe->legs[3].setDesiredy(rbe->legs[3].getDesiredy()
					+ (double) rbe->getVl() * length / (double) duty);
			rbe->legs[3].setDesiredz(rbe->legs[3].getDesiredz()
					- (double) (rbe->getHl()) * sin(i3 * (double) (PI / 180.0))
							* 2.0 / (double) duty * height);
			if (walking == 1) {
				//weight on body = 0
			} else {
				//weight on body = -264
			}
		}
		if ((gaitmotion < offset3) || (gaitmotion >= (offset3 + duty))) {
			rbe->legs[3].setDesiredx(rbe->legs[3].getDesiredx()
					- (double) rbe->getBl() * width / ((double) offset4
							- (double) duty)); // Calculates the desired x
			rbe->legs[3].setDesiredy(rbe->legs[3].getDesiredy()
					- (double) rbe->getVl() * length / ((double) offset4
							- (double) duty));
			rbe->legs[3].setDesiredz(rbe->legs[3].getDesiredz());
			//weight on body = -264
		}
	}

    	Robot* getRobot(){ return rbe; }

    	void printDesiredPositions() {
		cout << "Leg 0, X: " << rbe->legs[0].getDesiredx() << " Y: "
				<< rbe->legs[0].getDesiredy() << " Z: "
				<< rbe->legs[0].getDesiredz() << endl;
		cout << "Leg 1, X: " << rbe->legs[1].getDesiredx() << " Y: "
				<< rbe->legs[1].getDesiredy() << " Z: "
				<< rbe->legs[1].getDesiredz() << endl;
		cout << "Leg 2, X: " << rbe->legs[2].getDesiredx() << " Y: "
				<< rbe->legs[2].getDesiredy() << " Z: "
				<< rbe->legs[2].getDesiredz() << endl;
		cout << "Leg 3, X: " << rbe->legs[3].getDesiredx() << " Y: "
				<< rbe->legs[3].getDesiredy() << " Z: "
				<< rbe->legs[3].getDesiredz() << endl;
	}

    	void logDesiredPositions() {
		ofstream mylog;

		/*
		 int t2s=Gait::getType();
		 string s;
		 stringstream out;
		 out<<t2s<<"DesirePositionLog";
		 s=out.str();*/
		if (logbool == 0) {
			mylog.open("logDesiredPositions.txt", ios::out | ios::trunc);
			logbool = 1;
		} else {
			mylog.open("logDesiredPositions.txt", ios::out | ios::app);
		}

		mylog << rbe->legs[0].getDesiredx() << endl << rbe->legs[0].getDesiredy()
				<< endl << rbe->legs[0].getDesiredz() << endl
				<< rbe->legs[1].getDesiredx() << endl << rbe->legs[1].getDesiredy()
				<< endl << rbe->legs[1].getDesiredz() << endl
				<< rbe->legs[2].getDesiredx() << endl << rbe->legs[2].getDesiredy()
				<< endl << rbe->legs[2].getDesiredz() << endl
				<< rbe->legs[3].getDesiredx() << endl << rbe->legs[3].getDesiredy()
				<< endl << rbe->legs[3].getDesiredz() << endl;
		mylog.close();
	}

    	int getLogbool() const { return logbool; }
   	void setLogbool(int logbool) { this->logbool = logbool; }
};

#endif /* GAIT_H_ */
